package com.xx.douyi.service.impl;

public class GPGGAConverter {

    // WGS-84椭球体参数
    private static double a = 6378137;
    private static double b = 6356752.314245;

    // 大地坐标系中的参数
    private static double e = Math.sqrt((a * a - b * b) / (a * a));
    private static double ep = Math.sqrt((a * a - b * b) / (b * b));
    
    /**
     * 将GPGGA格式中的经纬度转换为十进制度数形式
     * Example：4807.038,N => 48.1173°N
     */
    public static double convertDMStoDegree(String dms) {
        String[] parts = dms.split("\\.");
        String degreeString = parts[0].substring(0, parts[0].length() - 2);
        String minuteString = parts[0].substring(parts[0].length() - 2);

        double secondsAndFraction = Double.parseDouble("0." + parts[1]) * 60;
        double minutes = Double.parseDouble(minuteString) + secondsAndFraction / 60;

        double degree;
        if (degreeString.charAt(0) == '0') {
            degree = Double.parseDouble(degreeString.substring(1)) + minutes / 60;
        } else {
            degree = Double.parseDouble(degreeString) + minutes / 60;
        }

        return degree;
    }

    /**
     * 计算对应经纬度在WGS84椭球体大地坐标系中的坐标
     */
    public static double[] convertToGeodeticCoordinate(double latitude, double longitude) {
        double lambda = Math.toRadians(longitude);
        double phi = Math.toRadians(latitude);

        // 计算相关参数
        double sin_phi = Math.sin(phi);
        double cos_phi = Math.cos(phi);
        double sin_lambda = Math.sin(lambda);
        double cos_lambda = Math.cos(lambda);
        double v = a / Math.sqrt(1 - e * e * sin_phi * sin_phi);

        // 计算X,Y,Z坐标
        double x = (v + 0) * cos_phi * cos_lambda;
        double y = (v + 0) * cos_phi * sin_lambda;
        double z = (v * (1 - e * e) + 0) * sin_phi;

        double[] result = new double[3];
        result[0] = x;
        result[1] = y;
        result[2] = z;

        return result;
    }

    /**
     * 计算对应高度
     */
    public static double convertMSLtoEllipsoidalHeight(double latitude, double longitude, double msl) {
        double[] p = convertToGeodeticCoordinate(latitude, longitude);
        double phi = Math.atan(p[2] / Math.sqrt(p[0] * p[0] + p[1] * p[1]));
        double N = a / Math.sqrt(1 - e * e * Math.sin(phi) * Math.sin(phi));
        double h = msl + p[2] - N;
        return h;
    }

    /**
     * 转换方法
     */
    public static void main(String[] args) {
        String latitudeString = "4250.5589,S";
        String longitudeString = "14718.5084,E";
        double msl = 100.0;

        double latitude = convertDMStoDegree(latitudeString.substring(0, latitudeString.length() - 2));
        if (latitudeString.charAt(latitudeString.length()-1) == 'S') {
            latitude = -latitude; // 南半球为负数
        }
        double longitude = convertDMStoDegree(longitudeString.substring(0, longitudeString.length() - 2));
        if (longitudeString.charAt(longitudeString.length()-1) == 'W') {
            longitude = -longitude; // 西经为负数
        }

        double[] geoCoord = convertToGeodeticCoordinate(latitude, longitude);
        double ellipsoidalHeight = convertMSLtoEllipsoidalHeight(latitude, longitude, msl);

        System.out.println("Latitude: " + latitude);
        System.out.println("Longitude: " + longitude);
        System.out.println("Ellipsoidal Height: " + ellipsoidalHeight);
        for (double v : geoCoord) {
            System.out.println(v);
        }
    }

}
